from global_var import *
from All_Init import *
import global_var
import math
class PID():
    def __init__(self, Kp, Ki, Kd, Kp2=0, Kd2=0, PD_limit=0, I_limit=0,Output_limit=0,Lower_limit=None):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.Kd2 = Kd2
        self.Kp2 = Kp2
        self.Integral = 0
        self.Integral_Max = 600000
        self.Last_Error = 0
        self.Output_limit = Output_limit
        self.Lower_limit = -Output_limit if Lower_limit is None else Lower_limit

        self.P_output = 0
        self.I_output = 0
        self.D_output = 0
        self.PD_Output = 0
        self.PD_limit = PD_limit
        self.I_limit = I_limit
        self.Output = 0
        self.dynamic_p = 0
        self.dynamic_d = 0
        self.static_p = 0
        self.static_d = 0
        self.Loc_Output = 0
        self.last_d = 0

    def Limit(self, Result, Limit):
        if Limit != 0:
            if Result > Limit:
                return Limit
            elif Result < self.Lower_limit:
                return self.Lower_limit
            else:
                return Result
        else:
            return Result

    def Loc_PID(self, error, Kp2_flag=0):
        self.Integral += error
        self.Integral = self.Limit(self.Integral, self.Integral_Max)
        self.I_output = self.Ki * self.Integral
        self.P_output = self.Kp * error
        if Kp2_flag:
            self.P_output = self.P_output + self.Kp2 * error * error
        if self.Kd2 == 0:
            self.D_output = self.Kd * (error - self.Last_Error)
        else:
            self.D_output = self.Kd * (error - self.Last_Error) + self.Kd2 * global_var.Gyro['gx'] 
        self.Last_Error = error
        if(self.PD_limit == 0):
            self.PD_Output = self.P_output + self.D_output
        else:
            self.PD_Output = self.Limit(self.P_output + self.D_output, self.PD_limit)
        self.Output = self.Limit(self.PD_Output + self.I_output, self.Output_limit)
        
        return self.Output
    def Loc_Turn_PID(self,error,Aim_speed,Real_speed,Dynamic_flag = 0):
        self.Integral += error
        self.Integral = self.Limit(self.Integral, self.Integral_Max)
        self.I_output = self.Ki * self.Integral
        if Dynamic_flag:
            self.Kp = self.Dynamic_Cal_P(abs(global_var.Speed_L-global_var.Speed_R))
        self.P_output = self.Kp * error
        if Dynamic_flag:
            self.Kd = self.Dynamic_Cal_D(error)
        self.D_output = self.Kd * (self.last_d * 0.8 + (error - self.Last_Error) * 0.2)
        self.last_d = error - self.Last_Error
        if self.Kd2 != 0:
            self.D_output += self.Kd2 * global_var.Gyro['gz']
        self.D_output = self.Limit(self.D_output,7500)
        self.Last_Error = error
        if(self.PD_limit == 0):
            self.PD_Output = self.P_output + self.D_output
        else:
            self.PD_Output = self.Limit(self.P_output + self.D_output, self.PD_limit)
        self.Output = self.Limit(self.PD_Output + self.I_output, self.Output_limit)
        return self.Output
    def Speed_Loc_PID(self, error):
        self.Integral += error
        self.Integral = self.Limit(self.Integral, 2000)
        self.I_output = self.Ki * self.Integral
        self.P_output = self.Kp * error
        self.D_output = self.Kd * (error - self.Last_Error)
        self.Last_Error = error
        if(self.PD_limit == 0):
            self.PD_Output = self.P_output + self.D_output
        else:
            self.PD_Output = self.Limit(self.P_output + self.D_output, self.PD_limit)
        self.Output = self.Limit(self.PD_Output + self.I_output, self.Output_limit)
        return self.Output
    def Dynamic_Cal_P(self, Param):
        return Param * self.dynamic_p + self.static_p
    def Dynamic_Cal_D(self, Param): 
        return Param * Param * self.dynamic_d + self.static_d

def Flash_Write(Params):
    try:
        user_file = io.open("user_data.txt", "r+")
    except:
        print("user_data.txt 文件不存在，新建该文件.")
        user_file = io.open("user_data.txt", "w+")
        
    user_file.seek(0, 0)
    for Param in Params:
        user_file.write("%f\n" % (Param))
    user_file.flush()
    user_file.close()

def Write_Param_Update():
    Write_Param_lst = []
    print(global_var.keys_list)
    print(global_var.Param_dict.items())
    
    for key in global_var.keys_list:
        Write_Param_lst.append(global_var.Param_dict[key])
    print(Write_Param_lst)
    Flash_Write(Write_Param_lst)
    
def PID_Init():
    global Acc_PID
    global Angle_PID
    global Turn_PID
    global Speed_PID
    global Acc_Turn_PID
    Acc_PID.Kp = global_var.Param_dict["AccKp"]
    Acc_PID.Ki = global_var.Param_dict["AccKi"]
    Acc_PID.Kd = global_var.Param_dict["AccKd"]
    
    Angle_PID.Kp = global_var.Param_dict["AnKp"]
    Angle_PID.Ki = global_var.Param_dict["AnKi"]
    Angle_PID.Kd = global_var.Param_dict["AnKd"]
    
    Turn_PID.dynamic_p = global_var.Param_dict["dy_p"]
    Turn_PID.static_p = global_var.Param_dict["st_p"]
    Turn_PID.static_d = global_var.Param_dict["st_d"]
    Turn_PID.static_d2 = global_var.Param_dict["st_d2"]
    
    Acc_Turn_PID.Kp = global_var.Param_dict["Ac_TKp"]
    Acc_Turn_PID.Kd = global_var.Param_dict["Ac_TKd"]
    
    Speed_PID.Kp = global_var.Param_dict["sKp"]
    Speed_PID.Ki = global_var.Param_dict["sKi"]
    Speed_PID.Kd = global_var.Param_dict["sKd"]
# ------------- PID参数 ------------- # 
Turn_PID = PID(0, 0, 0,Kp2=0,PD_limit=0,Output_limit=5000)
Acc_Turn_PID = PID(0, 0, 0,PD_limit=0,Output_limit=3300)

Acc_PID = PID(0, 0, 0, PD_limit=0, Output_limit=9000)
Angle_PID = PID(0, 0, 0,PD_limit=0,Output_limit=7000)
Speed_PID = PID(0, 0, 0, PD_limit=0, Output_limit=50,Lower_limit=-500)
Write_Param_lst = []